﻿using System;
using System.Collections.Generic;

using OpenTK;

using AnimatingHair.Auxiliary;

namespace AnimatingHair.Entity
{
    class Hair
    {
        public double H1, H2;

        public HairParticle[] Particles = new HairParticle[ Const.ParticleCount ];
        public SparseMatrix<ParticlePair> ParticlePairs = new SparseMatrix<ParticlePair>();

        private Scene scene;

        public Hair( Scene scene )
        {
            this.scene = scene;
        }

        private double[] alpha_r = new double[ Const.ParticleCount ];
        private double[] alpha_t = new double[ Const.ParticleCount ];

        public void CalculateForces()
        {
            // 0. update positions in voxel grid ?? TODO
            updateVoxelGrid();

            // 1. neighbor search
            neighborSearchVoxelGrid();

            // 2. calculate density
            calculateDensity();

            // 3. apply forces
            foreach ( HairParticle hp in Particles )
            {
                prepareParticle( hp );
            }

            foreach ( ParticlePair pp in ParticlePairs )
            {
                pp.Alpha = findAlpha_ij( pp.Particle1.ID, pp.Particle2.ID );
                pp.T = findT_ij( pp );
            }

            //calculateT_ij();   // TODO: ???

            applySpringForces();

            applyNeighborForces();

            applyAirFrictionForces();

            foreach ( HairParticle hp in Particles )
            {
                if ( hp.IsRoot )
                {
                    hp.Force = Vector3d.Zero;
                }
            }
        }

        public void UpdateDirections()
        {
            for ( int i = 0; i < Particles.Length; i++ )
            {
                updateDirection( Particles[ i ] );
            }
        }

        public void IntegrateForces( double timeStep )
        {
            foreach ( HairParticle hp in Particles )
            {
                hp.IntegrateForce( timeStep );
            }
        }

        private void updateVoxelGrid()
        {
            foreach ( HairParticle hp in Particles )
            {
                scene.VoxelGrid.UpdateParticle( hp );
            }
        }

        public Vector3d CenterPosition()
        {
            Vector3d sum = Vector3d.Zero;

            foreach ( HairParticle hp in Particles )
            {
                sum += hp.Position;
            }

            return sum / Const.ParticleCount;
        }

        #region Private step methods

        private void applyAirFrictionForces()
        {
            for ( int i = 0; i < Particles.Length; i++ )
            {
                HairParticle hp = Particles[ i ];

                Vector3d direction = hp.Velocity;
                double velocityMagnitude = direction.Length;

                if ( velocityMagnitude < 0.000001 )
                    continue;

                direction = direction / velocityMagnitude; // normalize

                hp.Force -= (Const.AirFriction * velocityMagnitude) * direction;
            }
        }

        private void applyNeighborForces()
        {
            for ( int i = 0; i < Particles.Length; i++ )
            {
                HairParticle hp = Particles[ i ];

                for ( int j = 0; j < hp.Neighbors.Count; j++ )
                {
                    HairParticle neighbor = hp.Neighbors[ j ];

                    Vector3d f = calculateHairHairForces( hp, neighbor );

                    hp.Force += f;
                }
            }
        }

        private Vector3d calculateHairHairForces( HairParticle hp_i, HairParticle hp_j )
        {
            Vector3d x_ij = hp_j.Position - hp_i.Position;
            Vector3d x_ji = -x_ij;
            double x_ijLength = x_ij.Length;

            Vector3d v_ij = hp_j.Velocity - hp_i.Velocity;

            Vector3d d_n = Vector3d.Cross( hp_i.Direction, hp_j.Direction );
            if ( d_n.Length < 0.05 ) //TODO: arbitrarne zvolena konstanta
            {
                d_n = x_ij - (Vector3d.Dot( x_ij, hp_i.Direction ) * hp_i.Direction);
            }
            d_n.Normalize();

            // ATTRACTION/REPULUSION FORCE
            double P_i = Const.k_a * (hp_i.Density - Const.rho_0);
            double P_j = Const.k_a * (hp_j.Density - Const.rho_0);

            //TODO: spravne urobit derivaciu v smere
            Vector3d wgrad = Const.Wgrad( hp_i.Position, hp_j.Position, H2 );
            double d1 = P_i / (hp_i.Density * hp_i.Density);
            double d2 = P_j / (hp_j.Density * hp_j.Density);
            Vector3d f_a = -hp_i.Mass * hp_j.Mass * (d1 + d2) * wgrad;
            //Vector3d f_a = -hp_i.Mass * hp_j.Mass * (P_i / (hp_i.Density * hp_i.Density) + P_j / (hp_j.Density * hp_j.Density)) * Const.Wgrad2(x_ij, H2);
            //Vector3d f_a = Vector3d.Zero; // zatial to nefunguje dobre

            // COLLISION FORCE
            Vector3d f_c = Vector3d.Zero;
            double sign = (Vector3d.Dot( x_ij, d_n ) * Vector3d.Dot( v_ij, d_n ));
            if ( sign < 0 )
                f_c = Const.d_c * Const.W( x_ijLength, H2 ) * Vector3d.Dot( v_ij, d_n ) * d_n;

            // FRICTION FORCE
            Vector3d f_f = Vector3d.Zero;
            Vector3d d_t = v_ij - (Vector3d.Dot( v_ij, d_n ) * d_n);
            if ( !(d_t.X < 0.00001 && d_t.X < 0.00001 && d_t.X < 0.00001) )
            {
                d_t.Normalize();
                f_f = Const.d_f * Const.W( x_ijLength, H2 ) * (Vector3d.Dot( v_ij, d_t ) * d_t);
            }

            return f_a + f_c + f_f;
        }

        private void prepareParticle( HairParticle hp )
        {
            hp.Force = Vector3d.Zero;

            calculateNewKX( hp );

            calculateAlphas( hp );

            // TODO: gravity
            hp.Force.Y -= Const.Gravity;

            // --- testing purposes ---
            //hp.Force.X = 2 * (rn.NextDouble() - 0.5);
            //hp.Force.Y = 2 * (rn.NextDouble() - 0.5);
            //hp.Force.Z = 2 * (rn.NextDouble() - 0.5);

            //if ( hp.IsRoot )
            //    hp.Position.X += 0.01;

            //hp.Force.X += 0.005;
        }

        private Vector3d findT_ij( ParticlePair pp )
        {
            Vector3d x_ijNormalized = pp.X;
            x_ijNormalized.Normalize();

            Vector3d axis = Vector3d.Cross( pp.Particle1.Direction, x_ijNormalized );

            return rotateVectorAroundAxis( x_ijNormalized, axis, pp.Theta );
        }

        private void updateDirection( HairParticle hp )
        {
            Vector3d sum = Vector3d.Zero;

            foreach ( HairParticle nhp in hp.NeighborsRoot )
            {
                sum += ParticlePairs[ hp, nhp ].Alpha * ParticlePairs[ hp, nhp ].K * ParticlePairs[ hp, nhp ].T;
            }

            foreach ( HairParticle nhp in hp.NeighborsTip )
            {
                sum += ParticlePairs[ hp, nhp ].Alpha * ParticlePairs[ hp, nhp ].K * ParticlePairs[ hp, nhp ].T;
            }

            sum.Normalize();

            hp.Direction = double.IsNaN( sum.X ) ? Vector3d.Zero : sum;
        }

        private static Vector3d rotateVectorAroundAxis( Vector3d v, Vector3d axis, double angle )
        {
            axis.Normalize();
            double sin = Math.Sin( angle );
            double cos = Math.Cos( angle );

            double x = axis.X;
            double y = axis.Y;
            double z = axis.Z;

            Vector3d a, b, c;
            a.X = 1 + (1 - cos) * (x * x - 1);
            a.Y = z * sin + (1 - cos) * x * y;
            a.Z = -y * sin + (1 - cos) * x * z;

            b.X = -z * sin + (1 - cos) * x * y;
            b.Y = 1 + (1 - cos) * (y * y - 1);
            b.Z = x * sin + (1 - cos) * y * z;

            c.X = y * sin + (1 - cos) * x * z;
            c.Y = -x * sin + (1 - cos) * y * z;
            c.Z = 1 + (1 - cos) * (z * z - 1);

            Vector3d result;
            result.X = Vector3d.Dot( v, a );
            result.Y = Vector3d.Dot( v, b );
            result.Z = Vector3d.Dot( v, c );

            return result;
        }

        private static double calculateA( double theta_ij, double theta_ji )
        {
            if ( Math.Cos( theta_ij ) * Math.Cos( theta_ji ) < 0 )
            {
                return Math.Abs( Math.Cos( theta_ij ) - Math.Cos( theta_ji ) ) / 2.0;
            }
            else
            {
                return 0;
            }
        }

        private void calculateT_ij()
        {
            // TODO
            throw new NotImplementedException();
            //foreach ( ParticlePair pp in ParticlePairs )
            //{
            //    double abc = pp.Alpha;
            //}
        }

        private void calculateAlphas( HairParticle hp )
        {
            double numerator = Const.E * hp.Area;
            double denominator = 0;

            foreach ( HairParticle neighbor in hp.NeighborsRoot )
            {
                ParticlePair pp = ParticlePairs[ hp, neighbor ];
                denominator += pp.L * pp.K;
            }
            if ( denominator != 0 )
                alpha_r[ hp.ID ] = numerator / denominator;

            denominator = 0;
            foreach ( HairParticle neighbor in hp.NeighborsTip )
            {
                ParticlePair pp = ParticlePairs[ hp, neighbor ];
                denominator += pp.L * pp.K;
            }
            if ( denominator != 0 )
                alpha_t[ hp.ID ] = numerator / denominator;
        }

        private void applySpringForces()
        {
            foreach ( ParticlePair pp in ParticlePairs )
            {
                Vector3d deltaLocation = pp.Particle2.Position - pp.Particle1.Position;
                double distance = deltaLocation.Length;
                deltaLocation.Normalize();

                double springCoefficient = pp.Alpha * pp.K;
                double springFactor = springCoefficient * (distance - pp.L);
                Vector3d force = springFactor * deltaLocation;

                pp.Particle1.Force += force;
            }
        }

        //private void integrateForces( double timeStep )
        //{
        //    foreach ( HairParticle hp in Particles )
        //    {
        //        if ( !hp.IsRoot )
        //        {
        //            Vector3d Acceleration = hp.Force / hp.Mass;
        //            hp.Velocity += timeStep * Acceleration;
        //            hp.Position += timeStep * hp.Velocity;
        //        }
        //    }
        //}

        private double findAlpha_ij( int i, int j )
        {
            if ( belongsTo( Particles[ i ], Particles[ j ].NeighborsTip ) &&
                belongsTo( Particles[ j ], Particles[ i ].NeighborsRoot ) )
            {
                return (alpha_r[ i ] + alpha_t[ j ]) / 2;
            }

            if ( belongsTo( Particles[ i ], Particles[ j ].NeighborsRoot ) &&
                belongsTo( Particles[ j ], Particles[ i ].NeighborsTip ) )
            {
                return (alpha_r[ j ] + alpha_t[ i ]) / 2;
            }

            return 0;
        }

        private static bool belongsTo( HairParticle hp, List<HairParticle> list )
        {
            return list.Contains( hp ); // TODO: usorti si na zaciatku neighborsRoot&List a rob binary search
        }

        private void calculateDensity()
        {
            foreach ( HairParticle hp in Particles )
            {
                double sum = hp.Mass * Const.W( 0, H2 );

                foreach ( HairParticle neighbor in hp.Neighbors )
                {
                    // TODO: subject to optimization
                    sum += neighbor.Mass * Const.W( (neighbor.Position - hp.Position).Length, H2 );
                }

                hp.Density = sum;
            }
        }

        private void neighborSearch()
        {
            for ( int i = 0; i < Const.ParticleCount; i++ )
                for ( int j = i + 1; j < Const.ParticleCount; j++ )
                {
                    Particles[ i ].Neighbors.Clear();
                    Particles[ j ].Neighbors.Clear();
                }

            for ( int i = 0; i < Const.ParticleCount; i++ )
                for ( int j = i + 1; j < Const.ParticleCount; j++ )
                    if ( (Particles[ i ].Position - Particles[ j ].Position).Length < 2 * H2 )
                    {
                        Particles[ i ].Neighbors.Add( Particles[ j ] );
                        Particles[ j ].Neighbors.Add( Particles[ i ] );
                    }
        }

        private void neighborSearchVoxelGrid()
        {
            for ( int i = 0; i < Const.ParticleCount; i++ )
                for ( int j = i + 1; j < Const.ParticleCount; j++ )
                {
                    Particles[ i ].Neighbors.Clear();
                    Particles[ j ].Neighbors.Clear();
                }

            for ( int i = 0; i < Const.ParticleCount; i++ )
            {
                foreach ( HairParticle neighbor in scene.VoxelGrid.FindNeighbors( Particles[ i ] ) )
                {
                    if ( (Particles[ i ].Position - neighbor.Position).Length < 2 * H2 )
                    {
                        if ( Particles[ i ] != neighbor )
                            Particles[ i ].Neighbors.Add( neighbor );
                    }
                }
            }
        }

        private void calculateNewKX( HairParticle hp )
        {
            Vector3d x_ij;
            double l;

            foreach ( HairParticle neighbor in hp.NeighborsRoot )
            {
                ParticlePair currentPair = ParticlePairs[ hp, neighbor ];

                x_ij = neighbor.Position - hp.Position;
                l = x_ij.Length;
                currentPair.X = x_ij;
                currentPair.XLength = l;

                currentPair.K = currentPair.C * Const.W( l, H1 );
            }

            foreach ( HairParticle neighbor in hp.NeighborsTip )
            {
                ParticlePair currentPair = ParticlePairs[ hp, neighbor ];

                x_ij = neighbor.Position - hp.Position;
                l = x_ij.Length;
                currentPair.X = x_ij;
                currentPair.XLength = l;

                currentPair.K = currentPair.C * Const.W( l, H1 );
            }
        }

        #endregion

        internal void RKStep(int stepNumber)
        {
            for ( int i = 0; i < Particles.Length; i++ )
            {
                HairParticle hp = Particles[ i ];

                hp.RKStep( stepNumber );
            }
        }

        internal void ApplyForceOnRootParticles( Vector3d force )
        {
            foreach ( HairParticle particle in Particles )
            {
                if ( particle.IsRoot )
                    particle.Force = force;
            }
        }
    }
}
